#include "bsp.h"

#include "SW_I2C/sw_i2c.h"
#include "SW_I2C/sw_i2c_ex.h"

#include "arm_math.h"
/* 定义IO = 1和 0的代码  */
#define I2C_SDA_1()		LSM303_SDA_PORT->BSRR = LSM303_SDA_PIN
#define I2C_SDA_0()		LSM303_SDA_PORT->BSRR = LSM303_SDA_PIN << 16;

#define I2C_SCL_1()		LSM303_SCL_PORT->BSRR = LSM303_SCL_PIN
#define I2C_SCL_0()		LSM303_SCL_PORT->BSRR = LSM303_SCL_PIN << 16;

#define I2C_SDA_READ()  ((LSM303_SDA_PORT->IDR & LSM303_SDA_PIN) != 0)	/* 读SDA口线状态 */
#define I2C_SCL_READ()  ((LSM303_SCL_PORT->IDR & LSM303_SCL_PIN) != 0)	/* 读SCL口线状态 */

#define I2C_WR	0		/* 写控制bit */
#define I2C_RD	1		/* 读控制bit */

// I2C总线时序接口函数
static void I2C_SDA_LOW(void);
static void I2C_SDA_HIGH(void);
static void I2C_SCL_LOW(void);
static void I2C_SCL_HIGH(void);
static uint8_t I2C_SDA_READ_LEVEL(void);
static void I2C_SDA_SET_INPUT(void);
static void I2C_SDA_SET_OUTPUT(void);
static void I2C_Delay(uint32_t us);

/* Private variables ---------------------------------------------------------*/
// 定义i2c驱动对象
static sw_i2c_dev_t i2c_lsm303_dev;

static void lsm303_gpio_config(void)
{
    
}

static void aht21_i2c_config(void)
{
    i2c_lsm303_dev.name               = "i2c_lsm303";
    i2c_lsm303_dev.speed              = 2; /*! speed:105Hz */
    i2c_lsm303_dev.delay_us           = I2C_Delay;
    i2c_lsm303_dev.ops.sda_low        = I2C_SDA_LOW;
    i2c_lsm303_dev.ops.sda_high       = I2C_SDA_HIGH;
    i2c_lsm303_dev.ops.scl_low        = I2C_SCL_LOW;
    i2c_lsm303_dev.ops.scl_high       = I2C_SCL_HIGH;
    i2c_lsm303_dev.ops.sda_read_level = I2C_SDA_READ_LEVEL;
    i2c_lsm303_dev.ops.sda_set_input  = I2C_SDA_SET_INPUT;
    i2c_lsm303_dev.ops.sda_set_output = I2C_SDA_SET_OUTPUT;
    sw_i2c_init(&i2c_lsm303_dev); 
}

static void lsm303_write_reg(uint8_t reg_addr, uint8_t data)
{
    uint8_t i2c_addr = (reg_addr > 0x19) ?  LSM303_I2C_ADDR_A : LSM303_I2C_ADDR_M;
    sw_i2c_ex_write_byte(&i2c_lsm303_dev, i2c_addr, reg_addr, data);
}

static uint8_t lsm303_read_reg(uint8_t reg_addr)
{
    uint8_t data = 0;
    uint8_t i2c_addr = (reg_addr > 0x19) ?  LSM303_I2C_ADDR_A : LSM303_I2C_ADDR_M;
    sw_i2c_ex_read_byte(&i2c_lsm303_dev, i2c_addr, reg_addr, &data);
    return data;
}


static void lsm303_read_regs(uint8_t reg_addr, uint8_t *data, uint8_t len)
{
    uint8_t i;
    uint8_t i2c_addr = (reg_addr > 0x19) ?  LSM303_I2C_ADDR_A : LSM303_I2C_ADDR_M;
    for(i = 0; i < len; i++)
    {
        sw_i2c_ex_read_byte(&i2c_lsm303_dev, i2c_addr, reg_addr + i, &data[i]);
    }
}


void bsp_lsm303_init(void)
{
    // 引脚初始化
    lsm303_gpio_config();
    // i2c初始化
    aht21_i2c_config();

}

void bsp_lsm303_sleep(void)
{
    lsm303_write_reg(LSM303_MR_REG_M, 0x03);
    lsm303_write_reg(LSM303_CTRL_REG1_A, 0x0F);
}

void bsp_lsm303_wakeup(void)
{
    lsm303_write_reg(LSM303_MR_REG_M, 0x00);    // 磁场传感器连续转换模式
    lsm303_write_reg(LSM303_CTRL_REG1_A, 0x2F); // low power mode, 10Hz速度
}

/*************************************************************************************************************************
*function  	:	LSM303_ReadAcceleration()
*paraments	: int16_t *Xa, int16_t *Ya, int16_t *Z
*return     : None
*detail     :	to read the Acceleration data, but the real Acceleration should be divided by the unit(n LSB/A) 
*************************************************************************************************************************/
void bsp_lsm3030_get_acceleration(int16_t *xa, int16_t *ya, int16_t *za)
{
    uint8_t buff[6];
    int16_t temp;
    lsm303_read_regs(LSM303_OUT_X_L_A, buff, 6);

	temp = buff[1];
	temp <<= 8;
	temp |= buff[0];
	*xa = temp;
	
	temp = buff[3];
	temp <<= 8;
	temp |= buff[2];
	*ya = temp;
	
	temp = buff[5];
	temp <<= 8;
	temp |= buff[4];
	*za = temp;    
}

/*************************************************************************************************************************
*function  	:	LSM303_ReadMagnetic()
*paraments	: int16_t *Xm, int16_t *Ym, int16_t *Zm
*return     : None
*detail     :	to read the Magnetic data, but the real Magnetic should be divided by the unit(n LSB/M) 
*************************************************************************************************************************/
void bsp_lsm303_get_magnetic(uint16_t *xm, uint16_t *ym, uint16_t *zm)
{
    uint8_t buff[6];
    int16_t temp;
    lsm303_read_regs(LSM303_OUT_X_H_M, buff, 6);

	temp = buff[0];
	temp <<= 8;
	temp |= buff[1];
	*xm = temp;
	
	temp = buff[2];
	temp <<= 8;
	temp |= buff[3];
	*zm = temp;
	
	temp = buff[4];
	temp <<= 8;
	temp |= buff[5];
	*ym = temp;
}

/*************************************************************************************************************************
*function  	:	LSM303_ReadTemperature()
*paraments	: int16_t *Temp
*return     : None
*detail     :	to read the Temperature data, 
							the real Temperature already be divided by 8(8 LSB/deg)(not guaranteed, as the datasheet said) 
*************************************************************************************************************************/
void bsp_lsm303_get_temperature(int16_t *temperature)
{
    uint8_t buff[2];
    int16_t temp;
    sw_i2c_ex_read_bytes(&i2c_lsm303_dev, LSM303_I2C_ADDR_M, TEMP_OUT_H_M, buff, 2);
	temp = buff[0];
	temp <<= 8;
	temp |= buff[1];
	temp >>= 4;
	*temperature = temp / 3 + 0.5;//8 LSB/deg
}

/*************************************************************************************************************************
*function  	:	LSM303DLH_CalculationZAxisAngle()
*paraments	: int16_t Xa, int16_t Ya, int16_t Za
*return     : ZAxisAngle
*detail     :	
*************************************************************************************************************************/
int bsp_lsm303_calculate_ZAxisAngle(int16_t xa, int16_t ya, int16_t za)
{
    double A;
    float fx,fy,fz;
    // 计算角加速度的矢量模长 |A|=根号下(X*X+Y*Y+Z*Z)
    A = sqrt((int)xa*xa + (int)ya*ya + (int)za*za); 
    fx = xa/A;
    fy = ya/A;
    fz = za/A;

    // Z方向
    A = fx*fx + fy*fy;
    A = sqrt(A);
    A = (double)A/fz;
    A = atan(A);
    A = A*180/PI;
    if(A < 0)
    {
        A += 90;
        A = 0 - A;
    }
    else
    {
        A = 90 - A;
    }
    return A*100;
}

/*************************************************************************************************************************
*function  	:	LSM303DLH_CalculationZAxisAngle()
*paraments	: int16_t Xa, int16_t Ya, int16_t Za
*return     : XAxisAngle
*detail     :	
*************************************************************************************************************************/
int bsp_lsm303_calculate_XAxisAngle(int16_t xa, int16_t ya, int16_t za)
{
    double A;
    float fx,fy,fz;

    // 计算角加速度的矢量模长 |A|=根号下(X*X+Y*Y+Z*Z)
    A = sqrt((int)xa*xa + (int)ya*ya + (int)za*za);
    fx = xa/A;
    fy = ya/A;
    fz = za/A;

    // X方向
    A = fx*fx + fz*fz;
    A = sqrt(A);
    A = (double)A/fx;
    A = atan(A);
    A = A*180/PI;
    if(A < 0)
    {
        A += 90; // 向上为正
    }
    else
    {
        A = 90 - A; // 向下为负
        A = 0 - A;
    }
    return A*100;
}

/*************************************************************************************************************************
*function  	:	Azimuth_Calculate()
*paraments	: int16_t Xa, int16_t Ya, int16_t Za, int16_t Xm, int16_t Ym, int16_t Zm
*return     : Azimuth
*detail     :	to Calculate the Azimuth to find the direction
*************************************************************************************************************************/
float bsp_lsm303_calculate_Azimuth(int16_t xa, int16_t ya, int16_t za, int16_t xm, int16_t ym, int16_t zm)
{
    float pitch, roll, Hy, Hx, Azimuth; 
    pitch = atan2f(xa, sqrtf(ya * ya + za * za));
    roll = atan2f(ya, sqrtf(xa * xa + za * za));
    Hy = ym * cosf(roll) + xm * sinf(roll) * sinf(pitch) - zm * cosf(pitch) * sinf(roll);
    Hx = xm * cosf(pitch) + zm * sinf(pitch);
    Azimuth = atan2f(Hy, Hx)*180.0/PI;
    return Azimuth;
}


/***********************************************************************/


static void I2C_SDA_LOW(void)
{
    I2C_SDA_0();
}

static void I2C_SDA_HIGH(void)
{
    I2C_SDA_1();
}

static void I2C_SCL_LOW(void)
{
    I2C_SCL_0();
}

static void I2C_SCL_HIGH(void)
{
    I2C_SCL_1();
}

static uint8_t I2C_SDA_READ_LEVEL(void)
{
    return I2C_SDA_READ();
}

static void I2C_SDA_SET_INPUT(void)
{
    I2C_SDA_1(); // 释放SDA引脚

}

static void I2C_SDA_SET_OUTPUT(void)
{

}

static void I2C_Delay(uint32_t us)
{
    HAL_Delay_us(us);
}
